/***********************************************************
     Starter code for Assignment 3

     This code was originally written by Jack Wang for
		    CSC418, SPRING 2005

		implements scene_object.h

***********************************************************/

#include <cmath>
#include <iostream>
#include <stdlib.h>
#include "scene_object.h"
#include "../Materials/UnitSquareTextureMapper.h"

int nextId = 1;
UnitSquareTextureMapper defaultTextureMapper;

SceneObject::SceneObject(const Material* material)
: _shapeId(nextId++), _material(material)
{
}

int SceneObject::getShapeId() const
{
    return _shapeId;
}

Ray3D SceneObject::getRayInObjectSpace(const Ray3D& rayInWorldSpace,
                                       const Matrix4x4& worldToModel) const
{
    Ray3D rayInObjectSpace;
    rayInObjectSpace.origin = worldToModel * rayInWorldSpace.origin;
    rayInObjectSpace.dir = (worldToModel * rayInWorldSpace.dir);
    return rayInObjectSpace;
}

void SceneObject::addTransform(const Matrix4x4& transformation)
{
    _transformation = _transformation * transformation;
}

const Matrix4x4& SceneObject::getTransform() const
{
    return _transformation;
}

void SceneObject::addInvTransform(const Matrix4x4& invTransform)
{
    _invTransformation = invTransform * _invTransformation;
}

const Matrix4x4& SceneObject::getInvTransform() const
{
    return _invTransformation;
}

void SceneObject::setMaterial(const Material* material)
{
    _material = material;
}

const Material* SceneObject::getMaterial() const
{
    return _material;
}

void SceneObject::rotate(char axis, double angle)
{
	Matrix4x4 rotation;
	double toRadian = 2*M_PI/360.0;
	int i;
	
	for (i = 0; i < 2; i++) {
		switch(axis) {
			case 'x':
				rotation[0][0] = 1;
				rotation[1][1] = cos(angle*toRadian);
				rotation[1][2] = -sin(angle*toRadian);
				rotation[2][1] = sin(angle*toRadian);
				rotation[2][2] = cos(angle*toRadian);
				rotation[3][3] = 1;
                break;
			case 'y':
				rotation[0][0] = cos(angle*toRadian);
				rotation[0][2] = sin(angle*toRadian);
				rotation[1][1] = 1;
				rotation[2][0] = -sin(angle*toRadian);
				rotation[2][2] = cos(angle*toRadian);
				rotation[3][3] = 1;
                break;
			case 'z':
				rotation[0][0] = cos(angle*toRadian);
				rotation[0][1] = -sin(angle*toRadian);
				rotation[1][0] = sin(angle*toRadian);
				rotation[1][1] = cos(angle*toRadian);
				rotation[2][2] = 1;
				rotation[3][3] = 1;
                break;
		}
		if (i == 0) {
		    addTransform(rotation);
			angle = -angle;
		}
		else {
            addInvTransform(rotation);
		}
	}
}

void SceneObject::translate(Vector3D trans)
{
	Matrix4x4 translation;
	
	translation[0][3] = trans[0];
	translation[1][3] = trans[1];
	translation[2][3] = trans[2];
	addTransform(translation);
	translation[0][3] = -trans[0];
	translation[1][3] = -trans[1];
	translation[2][3] = -trans[2];
	addInvTransform(translation);
}

void SceneObject::scale(Point3D origin, double factor[3])
{
	Matrix4x4 scale;
	
	scale[0][0] = factor[0];
	scale[0][3] = origin[0] - factor[0] * origin[0];
	scale[1][1] = factor[1];
	scale[1][3] = origin[1] - factor[1] * origin[1];
	scale[2][2] = factor[2];
	scale[2][3] = origin[2] - factor[2] * origin[2];
    addTransform(scale);
	scale[0][0] = 1/factor[0];
	scale[0][3] = origin[0] - 1/factor[0] * origin[0];
	scale[1][1] = 1/factor[1];
	scale[1][3] = origin[1] - 1/factor[1] * origin[1];
	scale[2][2] = 1/factor[2];
	scale[2][3] = origin[2] - 1/factor[2] * origin[2];
    addInvTransform(scale);
}

// Returns the texture mapper for the shape
const ITextureMapper& SceneObject::getTextureMapper() const
{
    return defaultTextureMapper;
}

UnitSquare::UnitSquare(Material* material)
: SceneObject(material)
{
    
}

bool UnitSquare::intersect(Ray3D& ray,
                           const Matrix4x4& worldToModel,
                           const Matrix4x4& modelToWorld) const
{
	
    Ray3D rayInObjectSpace = getRayInObjectSpace(ray, worldToModel);
    
    if (rayInObjectSpace.dir.length() == 0)
    {
        return false;
    }
    
    Vector3D normal (0, 0, 1);
    
    // Check if ray is parallel to square and if so it does not intersect
    if (normal.dot(rayInObjectSpace.dir) == 0)
    {
        return false;
    }
    
    // Let ray be p + t * d where p = px, py, pz and d = dx, dy, dz
    // Then p = [px + t dx, py + t dy, pz + t dz]
    // We first find the point where z becomes 0, => pz + t dz = 0
    // If dz = 0 then t can have any value and in that case we pick t = 0, else t = -(pz) / dz
    
    double pz = rayInObjectSpace.origin[2];
    double dz = rayInObjectSpace.dir[2];
    double t = 0;
    
    if (dz != 0)
    {
        t = -pz / dz;
    }
    
    // Intersection happens behind the camera
    if (t < 0)
    {
        return false;
    }
    
    double x = rayInObjectSpace.origin[0] + t * rayInObjectSpace.dir[0];
    double y = rayInObjectSpace.origin[1] + t * rayInObjectSpace.dir[1];
        
    // Intersection 
    if (((x >= -0.5) && (x <= 0.5)) && ((y >= -0.5) && (y <= 0.5)))
    {
        Point3D intersectionPoint (x, y, 0);
        if ((ray.intersection.none) || (ray.intersection.t_value > t))
        {
            ray.intersection.none = false;
            ray.intersection.t_value = t;
            ray.intersection.normal = transNorm(worldToModel, normal);
            ray.intersection.normal.normalize();
            ray.intersection.point = modelToWorld * intersectionPoint;
            ray.intersection.pointInObjectSpace = intersectionPoint;
            ray.intersection.obj = this;
            return true;
        }
    }

	return false;
}

// Returns true if world point is contained within the object
bool UnitSquare::contains(const Point3D& worldPoint, const Matrix4x4& worldToModel) const
{
    Point3D point = worldToModel * worldPoint;
    return (point[3] == 0) && (((point[0] >= -0.5) && (point[0] <= 0.5)) && ((point[1] >= -0.5) && (point[1] <= 0.5)));
}

UnitSphere::UnitSphere(Material* material)
: SceneObject(material)
{
    
}

bool UnitSphere::intersect(Ray3D& ray,
                           const Matrix4x4& worldToModel,
                           const Matrix4x4& modelToWorld) const
{
    
    Ray3D rayInObjectSpace = getRayInObjectSpace(ray, worldToModel);
    
    if (rayInObjectSpace.dir.length() == 0)
    {
        return false;
    }
    
    // Let the sphere be at origin O and ray intersects the sphere at points a and b
    // where a is the point which is closer to the world origin.
    // Let l be a vector from ray's origin to the sphere's centre.
    // The distance from ray's origin to the mid point of a and b is just projection of l
    // on the direction vector of ray. Let this distance be l1.
    // Also let the distance between a and b to be 2c. Then l1 = | rayOrigin + t * rayDirection (unit) + c |
    // Let d be the distance between origin of sphere and mid point of a and b
    // We also know that c ^ 2 + d ^ 2  = 1
    // And d ^ 2 = l ^ 2 - l1 ^ 2
    // => c ^ 2 = 1 - (l ^ 2 - l1 ^ 2)
    // If c ^ 2 < 0 then ray does not intersect sphere (no solution)
    // Else, t is the minimum of l1 - c and l1 + c divided by the length of ray direction vector
    
    Point3D o (0, 0, 0);
    Vector3D l (o - rayInObjectSpace.origin);
    double l1 = l.dot(rayInObjectSpace.dir) / rayInObjectSpace.dir.length();
    
    if (l1 < 0)
    {
        return false;
    }
    
    double dSquare = l.dot(l) - pow(l1, 2);
    double cSquare = 1 - dSquare;
    
    if (cSquare < 0)
    {
        return false;
    }
    
    double t1 = l1 - sqrt(cSquare);
    double t2 = l1 + sqrt(cSquare);
    
    double t = (t1 < t2 ? t1 : t2) / rayInObjectSpace.dir.length();
    
    if (t < 0)
    {
        return false;
    }
    
    if ((ray.intersection.none) || (ray.intersection.t_value > t))
    {
        Point3D intersectionPoint (rayInObjectSpace.origin[0] + t * rayInObjectSpace.dir[0],
                                   rayInObjectSpace.origin[1] + t * rayInObjectSpace.dir[1],
                                   rayInObjectSpace.origin[2] + t * rayInObjectSpace.dir[2]);
        
        ray.intersection.none = false;
        ray.intersection.normal = transNorm(worldToModel, intersectionPoint - o);
        ray.intersection.normal.normalize();
        ray.intersection.point = modelToWorld * intersectionPoint;
        ray.intersection.pointInObjectSpace = intersectionPoint;
        ray.intersection.t_value = t;
        ray.intersection.obj = this;
        return true;
    }
    
	return false;
}

// Returns true if world point is contained within the object
bool UnitSphere::contains(const Point3D& worldPoint, const Matrix4x4& worldToModel) const
{
    Point3D point = worldToModel * worldPoint;
    
    // Ensure point is within reasonable error margin
    return ((pow(point[0], 2)) + (pow(point[1], 2)) + (pow(point[2], 2))) <= 1.005;
}
